
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    # 获取功能包路径
    pkg_share = get_package_share_directory('fruit_cartographer')
    
    # 启动参数配置
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    resolution = LaunchConfiguration('resolution', default='0.05')
    publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')
    
    # 配置文件路径
    cartographer_config_dir = LaunchConfiguration('cartographer_config_dir',
                                                  default=os.path.join(pkg_share, 'config'))
    configuration_basename = LaunchConfiguration('configuration_basename',
                                                 default='fruitbot_2d.lua')
    
    # RViz配置文件路径
    rviz_config_dir = os.path.join(pkg_share, 'config', 'cartographer.rviz')
    
    print(f"RViz配置文件: {rviz_config_dir}")

    return LaunchDescription([
        # 声明启动参数
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='true',
            description='使用仿真时间进行建图（用于Gazebo等仿真环境）'),
        
        DeclareLaunchArgument(
            'cartographer_config_dir',
            default_value=os.path.join(pkg_share, 'config'),
            description='配置文件目录的完整路径'),
        
        DeclareLaunchArgument(
            'configuration_basename',
            default_value='fruitbot_2d.lua',
            description='Cartographer配置文件名称'),
        
        DeclareLaunchArgument(
            'resolution',
            default_value='0.05',
            description='栅格地图分辨率'),
        
        DeclareLaunchArgument(
            'publish_period_sec',
            default_value='1.0',
            description='地图发布周期（秒）'),

        # Cartographer建图节点
        Node(
        package='cartographer_ros',
        executable='cartographer_node',
        name='cartographer_node',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time}],
            arguments=[
                '-configuration_directory', cartographer_config_dir,
                '-configuration_basename', configuration_basename
            ],
        remappings=[
            ('scan', 'scan'),
            ('imu', '/imu')
            ]),

        # 地图发布节点
        Node(
        package='cartographer_ros',
        executable='cartographer_occupancy_grid_node',
        name='cartographer_occupancy_grid_node',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time}],
            arguments=[
                '-resolution', resolution,
                '-publish_period_sec', publish_period_sec
            ]),

        # RViz可视化节点
        Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz_config_dir],
        parameters=[{'use_sim_time': use_sim_time}],
            output='screen'),
    ])

